
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "iot_pwm.h"
#include "hi_gpio.h"
#include "hi_io.h"
#include "hi_pwm.h"
#include "hi_time.h"
#include "hi_i2c.h"
#include "ssd1306/ssd1306.h"
#include "ssd1306/ssd1306_fonts.h"
#include "engine_control.h"

#include "mqtt_control.h"


#include "hi_wifi_api.h"
#include "lwip/ip_addr.h"
#include "lwip/netifapi.h"
#include "lwip/sockets.h"

#include "wifi_connecter.h"
#include "car_wifi_hotspot.h"

#include "kv_store.h"


#include "cJSON.h"

#define GPIO0 0 
#define GPIO1 1 
#define GPIO9 9
#define GPIO10 10



typedef enum {
    /*停止*/
    CAR_STATUS_STOP,

    /*GPIO前进*/
    CAR_STATUS_FORWARD,

    /*GPIO后退*/
    CAR_STATUS_BACK,

    /*GPIO左转*/
    CAR_STATUS_LEFT,

    /*GPIO右转*/
    CAR_STATUS_RIGHT,
    
    /*PWM前进*/
    CAR_STATUS_PWMFORWARD,

    /*PWM后退*/
    CAR_STATUS_PWMBACK,

    /*PWM左转*/
    CAR_STATUS_PWMLEFT,

    /*PWM右转*/
    CAR_STATUS_PWMRIGHT


} CarStatus;

struct car_sys_info {

    CarStatus cur_status;
    CarStatus go_status;
    char status_change;

} ;


struct car_sys_info car_info;

void gpio_init(void)
{
    printf("start car gpio_init!\n");

    IoTGpioInit(GPIO0);
    IoTGpioSetFunc(GPIO0,0);
    IoTGpioSetFunc(GPIO1,0);
    IoTGpioSetFunc(GPIO9,0);
    IoTGpioSetFunc(GPIO10,0);
    IoTGpioSetDir(GPIO0, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(GPIO1, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(GPIO9, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(GPIO10, IOT_GPIO_DIR_OUT);

}



//前进
void car_forward(void)
{
    // gpio_control(GPIO0,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    // gpio_control(GPIO1,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE0);
    // gpio_control(GPIO9,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    // gpio_control(GPIO10,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO0,IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO1,IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO9,IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO10,IOT_GPIO_VALUE0);
}

//后退
void car_back(void)
{
    // gpio_control(GPIO0,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE0);
    // gpio_control(GPIO1,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    // gpio_control(GPIO9,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE0);
    // gpio_control(GPIO10,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO0,IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO1,IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO9,IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO10,IOT_GPIO_VALUE1);
}

//左转
void car_left(void)
{
    // gpio_control(GPIO0,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    // gpio_control(GPIO1,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE0);
    // gpio_control(GPIO9,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    // gpio_control(GPIO10,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO0,IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO1,IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO9,IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO10,IOT_GPIO_VALUE1);
}

//右转
void car_right()
{
    
    // gpio_control(GPIO0,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    // gpio_control(GPIO1,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    // gpio_control(GPIO9,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    // gpio_control(GPIO10,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO0,IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO1,IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO9,IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO10,IOT_GPIO_VALUE0);
}



void car_stop(void)
{
    gpio_control(GPIO0,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    gpio_control(GPIO1,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    gpio_control(GPIO9,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    gpio_control(GPIO10,0,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    // IoTPwmStop(HI_IO_NAME_GPIO_0);
    // IoTPwmStop(HI_IO_NAME_GPIO_1);
    // IoTPwmStop(HI_IO_NAME_GPIO_9);
    // IoTPwmStop(HI_IO_NAME_GPIO_10);
    // IoTGpioSetOutputVal(GPIO0,IOT_GPIO_VALUE1);
    // IoTGpioSetOutputVal(GPIO1,IOT_GPIO_VALUE1);
    // IoTGpioSetOutputVal(GPIO9,IOT_GPIO_VALUE1);
    // IoTGpioSetOutputVal(GPIO10,IOT_GPIO_VALUE1);
}


void gpio_control(hi_gpio_idx id,int hifunc,IotGpioDir dir,IotGpioValue value)
{
    IoTGpioInit(id);
    IoTGpioSetFunc(id,hifunc);
    IoTGpioSetDir(id, dir);
    IoTGpioSetOutputVal(id,value);
}

void pwm_control(hi_io_name id,int hifunc,hi_pwm_port port,int duty)
{
    IoTGpioInit(id);
    IoTGpioSetFunc(id,hifunc); 
    IoTPwmInit(port);
    hi_pwm_start(port, duty, 45000);
}


void pwm_forward(void)
{
    gpio_control(HI_IO_NAME_GPIO_0,HI_IO_FUNC_GPIO_0_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    pwm_control(HI_IO_NAME_GPIO_1,HI_IO_FUNC_GPIO_1_PWM4_OUT,HI_PWM_PORT_PWM4,5500);
    gpio_control(HI_IO_NAME_GPIO_9,HI_IO_FUNC_GPIO_9_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    pwm_control(HI_IO_NAME_GPIO_10,HI_IO_FUNC_GPIO_10_PWM1_OUT,HI_PWM_PORT_PWM1,5500);
}


void pwm_left(void)
{
    gpio_control(HI_IO_NAME_GPIO_0,HI_IO_FUNC_GPIO_0_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    pwm_control(HI_IO_NAME_GPIO_1,HI_IO_FUNC_GPIO_1_PWM4_OUT,HI_PWM_PORT_PWM4,2150);
    gpio_control(HI_IO_NAME_GPIO_9,HI_IO_FUNC_GPIO_9_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    pwm_control(HI_IO_NAME_GPIO_10,HI_IO_FUNC_GPIO_10_PWM1_OUT,HI_PWM_PORT_PWM1,5500);
}


void pwm_right(void)
{
    gpio_control(HI_IO_NAME_GPIO_0,HI_IO_FUNC_GPIO_0_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    pwm_control(HI_IO_NAME_GPIO_1,HI_IO_FUNC_GPIO_1_PWM4_OUT,HI_PWM_PORT_PWM4,5500);
    gpio_control(HI_IO_NAME_GPIO_9,HI_IO_FUNC_GPIO_9_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    pwm_control(HI_IO_NAME_GPIO_10,HI_IO_FUNC_GPIO_10_PWM1_OUT,HI_PWM_PORT_PWM1,2150);
}

void pwm_back(void)
{
    gpio_control(HI_IO_NAME_GPIO_1,HI_IO_FUNC_GPIO_1_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    pwm_control(HI_IO_NAME_GPIO_0,HI_IO_FUNC_GPIO_0_PWM3_OUT,HI_PWM_PORT_PWM3,5500);
    gpio_control(HI_IO_NAME_GPIO_10,HI_IO_FUNC_GPIO_10_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    pwm_control(HI_IO_NAME_GPIO_9,HI_IO_FUNC_GPIO_9_PWM0_OUT,HI_PWM_PORT_PWM0,5500);
}


void pwm_stop(void)
{
    gpio_control(HI_IO_NAME_GPIO_1,HI_IO_FUNC_GPIO_1_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    gpio_control(HI_IO_NAME_GPIO_0,HI_IO_FUNC_GPIO_0_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    gpio_control(HI_IO_NAME_GPIO_9,HI_IO_FUNC_GPIO_9_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    gpio_control(HI_IO_NAME_GPIO_10,HI_IO_FUNC_GPIO_10_GPIO,IOT_GPIO_DIR_OUT,IOT_GPIO_VALUE1);
    
}

hi_float get_distance(void){
    hi_float distance = 0.0;
    static hi_u64 start_time =0,time =0;
    hi_gpio_value value =HI_GPIO_VALUE0;
    hi_u32 flag =0;

    IoTGpioSetFunc(HI_IO_NAME_GPIO_8,HI_IO_FUNC_GPIO_8_GPIO);
    IoTGpioSetDir(HI_GPIO_IDX_8,HI_GPIO_DIR_IN);
    IoTGpioSetDir(HI_GPIO_IDX_7,HI_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(HI_GPIO_IDX_7,HI_GPIO_VALUE1);
    hi_udelay(20);    
    IoTGpioSetOutputVal(HI_GPIO_IDX_7,HI_GPIO_VALUE0);
    
    while (1){
        IoTGpioGetInputVal(HI_GPIO_IDX_8,&value);
        if (value ==HI_GPIO_VALUE1 && flag ==0){
            start_time =hi_get_us();
            flag =1;
        }
        if (value == HI_GPIO_VALUE0 && flag ==1){
            hi_u64 endtime =hi_get_us();
            time = endtime-start_time;
            start_time = 0;
            break;
        }
    }
    distance =time * 0.034/2;
    return distance;
}


int connect_wifi(void)
{
    printf("connect_wifi\r\n");
    char *ssid[32], *key[32] ;
    strcpy(ssid,"");
    strcpy(key,"");
    UtilsGetValue("ssid",ssid,sizeof(ssid));
    UtilsGetValue("key",key,sizeof(key));
    if(strcmp(ssid,"")==0||strcmp(key,"")==0){
        printf("not found ssid and key. error connect_wifi\r\n");
        return -1;
    }
    WifiDeviceConfig apConfig = {0};
    strcpy(ssid,ssid);
    strcpy(key,key);

    strcpy(apConfig.ssid, ssid); // ssid HUAWEI-023E
    strcpy(apConfig.preSharedKey, key); // key 88888888  
    apConfig.securityType = WIFI_SEC_TYPE_PSK;

    int netId = ConnectToHotspot(&apConfig);
    if(netId==-1){
        // 未成功连接当前保存Wifi 即删除数据重新配网
        UtilsDeleteValue("ssid");
        UtilsDeleteValue("key");
    }
    printf("netId %d g_connected %d \r\n",netId,g_connected);
    printf("end connect_wifi\r\n");
    return netId;
}


void start_tcpsocket(void * argv)
{
    ssize_t retval = 0;
    struct sockaddr_in servaddr;    
    char recvline[1024];
    
    int backlog = 1;
    struct sockaddr_in clientAddr = {0};
    socklen_t clientAddrLen = sizeof(clientAddr);
    int connfd = -1;
    cJSON *recvjson;
    
    int sockfd = socket(PF_INET, SOCK_STREAM, 0); // tcp
 
    bzero(&servaddr, sizeof(servaddr));
    servaddr.sin_family = AF_INET;
    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    servaddr.sin_port = htons(40001);

    printf("Connect wifi start tcpsocket \r\n");
    bind(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr));
    retval = listen(sockfd, backlog); 

    while(1)
    {
        connfd = accept(sockfd, (struct sockaddr *)&clientAddr, &clientAddrLen);
        retval = recv(connfd, recvline, sizeof(recvline), 0);
        if (retval < 0) {
            printf("recv recvline failed, %ld!\r\n", retval);
        }
        printf("tcp recv recvline %s  from client done!\r\n", recvline);


        recvjson = cJSON_Parse(recvline);

        if(recvjson != NULL){
            
            char *type= cJSON_GetObjectItem(recvjson, "actionType")->valuestring;
            char *action= cJSON_GetObjectItem(recvjson, "action")->valuestring;
            car_run(type,action);

            cJSON_Delete(recvjson);
            
            strcpy(recvline,"tcp car run ");
            strcat(recvline,cJSON_GetObjectItem(recvjson, "action")->valuestring);
            retval = send(connfd, recvline, strlen(recvline), 0);
            if (retval < 0) {
                printf("send response failed, %ld!\r\n", retval);
            }
            printf("send response %s  to client done!\r\n", recvline);
            
        }
        close(connfd);
        // sleep(1);
    }
}

void start_udpsocket(void * argv)
{
    int ret;
    struct sockaddr_in servaddr;    
    char recvline[1024];
    cJSON *recvjson;
    

    int sockfd = socket(PF_INET, SOCK_DGRAM, 0);
 
    bzero(&servaddr, sizeof(servaddr));
    servaddr.sin_family = AF_INET;
    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    servaddr.sin_port = htons(50001);

    printf("Connect wifi start udpsocket \r\n");
    bind(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr));

    while(1)
    {
        struct sockaddr_in addrClient;
        int sizeClientAddr = sizeof(struct sockaddr_in);

        memset(recvline, sizeof(recvline), 0);
        ret = recvfrom(sockfd, recvline, 1024, 0, (struct sockaddr*)&addrClient,(socklen_t*)&sizeClientAddr);
        
        if(ret>0)
        {
            char *pClientIP =inet_ntoa(addrClient.sin_addr);
 
            printf("udp %s-%d(%d) says:%s\n",pClientIP,ntohs(addrClient.sin_port),addrClient.sin_port, recvline);
                                
            recvjson = cJSON_Parse(recvline);
            
            if(recvjson != NULL){
                char *type= cJSON_GetObjectItem(recvjson, "actionType")->valuestring;
                char *action= cJSON_GetObjectItem(recvjson, "action")->valuestring;
                car_run(type,action);
                strcpy(recvline,"udp car run ");
                strcat(recvline,action);
                ret = sendto(sockfd, recvline, strlen(recvline), 0, (struct sockaddr *)&addrClient, sizeof(addrClient));
                if (ret <= 0) {
                    printf("send failed, %ld!\r\n", ret);
                }
                printf("send message {%s} %ld done!\r\n", recvline, ret);
                cJSON_Delete(recvjson);
            }

        }
    }
}

void car_run(char *type,char *action){
    printf("car_run type %s , action %s !\r\n", type,action);
    if(strcmp(action,"forward")==0){
        set_car_status(strcmp(type,"0")==0?CAR_STATUS_FORWARD:CAR_STATUS_PWMFORWARD);
    }else if(strcmp(action,"back")==0){
        set_car_status(strcmp(type,"0")==0?CAR_STATUS_BACK:CAR_STATUS_PWMBACK);
    }else if(strcmp(action,"left")==0){
        set_car_status(strcmp(type,"0")==0?CAR_STATUS_LEFT:CAR_STATUS_PWMLEFT);
    }else if(strcmp(action,"right")==0){
        set_car_status(strcmp(type,"0")==0?CAR_STATUS_RIGHT:CAR_STATUS_PWMRIGHT);
    }else if(strcmp(action,"stop")==0){
        set_car_status(CAR_STATUS_STOP);
    }
}


void set_car_status(CarStatus status)
{
	if(status != car_info.go_status)
	{
		car_info.status_change = 1;
	}
	car_info.go_status = status;
    printf("set_car_status ,%d !\n",status);
}

void car_run_status(void)
{
    printf("start car_run_status!\n");
	while(1)
    {
        if(car_info.status_change){
            car_info.status_change=0;
            switch (car_info.go_status)
            {
                case CAR_STATUS_STOP:
                    car_stop();
    printf("car_status car_stop  %d !\n",car_info.go_status);
                    break;
                case CAR_STATUS_BACK:
    printf("car_status pwm_back  %d !\n",car_info.go_status);
                    pwm_back();
                    break;
                case CAR_STATUS_FORWARD:
    printf("car_status pwm_forward  %d !\n",car_info.go_status);
                    pwm_forward();
                    break;
                case CAR_STATUS_LEFT:
    printf("car_status pwm_left  %d !\n",car_info.go_status);
                    pwm_left();
                    break;
                case CAR_STATUS_RIGHT:
    printf("car_status pwm_right  %d !\n",car_info.go_status);
                    pwm_right();
                    break;
                case CAR_STATUS_PWMBACK:
    printf("car_status pwm_back2  %d !\n",car_info.go_status);
                    pwm_back();
                    break;
                case CAR_STATUS_PWMFORWARD:
    printf("car_status pwm_forward2  %d !\n",car_info.go_status);
                    pwm_forward();
                    break;
                case CAR_STATUS_PWMLEFT:
    printf("car_status pwm_left2  %d !\n",car_info.go_status);
                    pwm_left();
                    break;
                case CAR_STATUS_PWMRIGHT:
    printf("car_status pwm_right2  %d !\n",car_info.go_status);
                    pwm_right();    
                    break;                        
                default:
                    break;
            }
        }
        // 
        sleep(1);
    }
}



void oledshow(char msg){    
    // oled
    IoTGpioInit(HI_IO_NAME_GPIO_13);
    IoTGpioSetFunc(HI_IO_NAME_GPIO_13,HI_IO_FUNC_GPIO_13_I2C0_SDA);
    IoTGpioSetFunc(HI_IO_NAME_GPIO_14,HI_IO_FUNC_GPIO_14_I2C0_SCL);
    hi_i2c_init(HI_I2C_IDX_0,4000000);
    sleep(2);
    ssd1306_Init();
    ssd1306_Fill(Black);
    ssd1306_SetCursor(0, 0);
    ssd1306_DrawString(msg, Font_7x10, White);
    ssd1306_UpdateScreen();
}

void check_distance(void){
    static hi_float distance = 0,left_distance = 0,right_distance = 0;
    while(1){
        // 红外检测防止掉下桌子
        checkInfraRed();
        hi_udelay(20*1000);
        // 超声波检测障碍物避障
        distance = get_distance();
        // printf("show check_distance_thread %f!\n",distance);
        if(distance>0&&distance<=8&&car_info.go_status!=CAR_STATUS_STOP){
            car_info.cur_status = car_info.go_status;
            printf("show <=8 check_distance_thread %f!\n",distance);
            set_car_status(CAR_STATUS_PWMBACK);
            hi_udelay(800*1000);
            set_car_status(CAR_STATUS_STOP);
            engine_turn_left();
            hi_udelay(200*1000);
            left_distance = get_distance();
            engine_turn_right();
            hi_udelay(200*1000);
            right_distance = get_distance();
            regress_middle();
            if(left_distance>right_distance){
                set_car_status(CAR_STATUS_PWMLEFT);
                printf("gotoin check_distance_thread pwm_left %f!\n",left_distance);
            }else{
                set_car_status(CAR_STATUS_PWMRIGHT);
                printf("gotoin check_distance_thread pwm_right %f!\n",right_distance);
            }
            hi_udelay(800*1000);
            set_car_status(car_info.cur_status);
        }
        // hi_udelay(20*1000);
        sleep(1);
    }
}

// 红外检测 
void checkInfraRed(void){
    IotGpioValue rightValue=IOT_GPIO_VALUE0;
    IotGpioValue leftValue=IOT_GPIO_VALUE0;
    
    IoTGpioGetInputVal(HI_IO_NAME_GPIO_12,&leftValue);
    IoTGpioGetInputVal(HI_IO_NAME_GPIO_11,&rightValue);
    // 红外检测没有返回高电平信号
    if((leftValue==IOT_GPIO_VALUE0||rightValue==IOT_GPIO_VALUE0)&&car_info.go_status!=CAR_STATUS_STOP){ 
        car_info.cur_status = car_info.go_status;
        printf("goto in checkInfraRed %d leftValue %d rightValue %d !\n",car_info.cur_status,leftValue,rightValue);
        set_car_status(CAR_STATUS_PWMBACK);
        sleep(1);
        set_car_status(CAR_STATUS_STOP);
        hi_udelay(2000);
        set_car_status(car_info.cur_status);
    }
    
}

void check_distance_thread(void){
    printf("start check_distance_thread!\n");
    osThreadAttr_t attr;

    attr.name = "check_distance_thread";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 4096;
    attr.priority = 9;

    if (osThreadNew((osThreadFunc_t)check_distance, NULL, &attr) == NULL) {
        printf("Falied to create check_distance_thread!\n");
    }
}


void tcp_thread(void){
    printf("start tcp_thread!\n");
    osThreadAttr_t attr;

    attr.name = "tcp_thread";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 4096;
    attr.priority = 9;

    if (osThreadNew((osThreadFunc_t)start_tcpsocket, NULL, &attr) == NULL) {
        printf("Falied to create tcp_thread!\n");
    }
}

void udp_thread(void){
    printf("start udp_thread!\n");
    osThreadAttr_t attr;

    attr.name = "udp_thread";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 4096;
    attr.priority = 9;

    if (osThreadNew((osThreadFunc_t)start_udpsocket, NULL, &attr) == NULL) {
        printf("Falied to create udp_thread!\n");
    }
}


void car_status_thread(void){
    printf("start car_status_thread!\n");
    osThreadAttr_t attr;

    attr.name = "car_status_thread";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 4096;
    attr.priority = 9;

    if (osThreadNew((osThreadFunc_t)car_run_status, NULL, &attr) == NULL) {
        printf("Falied to create car_status_thread!\n");
    }
}


void car_thread(void * argv)
{
    printf("car_thread!\n");
    // gpio_init();
    int netid= connect_wifi();
    if(netid>-1){
        start_control();
    }else{
        WifiHotspot();
        start_dmssocket(); // 配网监听
        printf("end dmssocket!\n");
        connect_wifi();
        StopHotspot(); // 配网成功 关闭热点
        start_control();
    }
}

void start_control(void){
    tcp_thread();
    udp_thread();
    set_car_status(CAR_STATUS_STOP);
    check_distance_thread();
    car_status_thread();
    mqtt_Connect();
}

void CarControlEntry(void)
{
    printf("start CarControlEntry!\n");
    osThreadAttr_t attr;

    attr.name = "car";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 4096;
    attr.priority = 9;

    printf("osThreadNew CarControlEntry!\n");
    if (osThreadNew((osThreadFunc_t)car_thread, NULL, &attr) == NULL) {
        printf("Falied to create CarControlEntry!\n");
    }

}

SYS_RUN(CarControlEntry);